from plants import Plant
import numpy as np
import matplotlib.pyplot as plt

plant = Plant()
plott, plotf1, plotf2, plotg = [], [], [], []
theta = np.array([[2.], [-1.], [-0.]])
P = np.eye(3)*1e6
lamb = 0.99
u = 0.
yk1, yk2 = 0, 0
for n in range(50):
    # u = 10.
    u = 0.97*u + 0.3
    # u = 10*np.cos(0.1*n*np.pi)
    # u = 10*np.sign(np.cos(0.1*n*np.pi))
    plant.Set_Input(u)
    plant.Simulate(50)
    y = plant.Get_Output()
    x = np.array([[yk1], [yk2], [u]])
    K = np.matmul(P, x) / (lamb + np.matmul(x.T, np.matmul(P, x)))
    P = 1/lamb * np.matmul((np.eye(3) - np.matmul(K, x.T)), P)
    theta += K*(y - np.matmul(x.T, theta))
    yk2, yk1 = yk1, y
    plott.append(n*0.05)
    plotf1.append(theta[0, 0])
    plotf2.append(theta[1, 0])
    plotg.append(theta[2, 0])
print('finished')
plt.plot(plott, plotf1)
plt.plot(plott, plotf2)
plt.plot(plott, plotg)
plt.show()
